/**
  *****************************************************************************
  * @Author       : JackyJuu
  * @Site         : https://www.geekros.com
  * @LastEditTime : 2022-03-18 18:05:09
  * @FilePath     : \Geek_Dog\hardware\robot\lite_dog\src\dog_remote.c
  * @Description  : 机器人遥控相关代码
  * @Copyright (c) 2022 by GEEKROS, All Rights Reserved. 
  ******************************************************************************
*/
#include "dog_remote.h"
#include "dog_move.h"
#include "dog_app.h"



/*******************************************************************************
  * @funtion      : Dog_Remote_Motion_Data_Get  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗遥控器运动相关数据设置
  * @param         {Dog_Control_Data_Struct*} Move_Data_Get
  * @param         {Dog_Move_Mode_Enum*} Move_Mode_Set
  * @param         {float*} Move_Height_Get
  * @param         {float*} Move_Lenth_Get
  * @param         {float*} Move_Speed_Get
  * @return        {*}
  *******************************************************************************/
void Dog_Remote_Motion_Data_Get(Dog_Control_Data_Struct* Move_Data_Get,Dog_Move_Mode_Enum* Move_Mode_Set,float* Move_Height_Get,float* Move_Lenth_Get,float* Move_Speed_Get)
{
	float Last_Speed_Control;
	//前后移动
	if(Move_Data_Get->Dog_Remote_Control->ch3 < -Dog_Move_Control_Error*2)
	{
		*Move_Mode_Set = Back_M;
		*Move_Speed_Get = fabs((float)Move_Data_Get->Dog_Remote_Control->ch3 / 80.00f * Dog_Move_Max_Speed);
//		if()
		*Move_Lenth_Get = (Dog_Move_Max_Lenth - (1.00f - fabs((float)Move_Data_Get->Dog_Remote_Control->ch3 / 80.00f)) * Dog_Move_Max_Control_Lenth);
		*Move_Height_Get = (Dog_Move_Max_Height - (1.00f - fabs((float)Move_Data_Get->Dog_Remote_Control->ch3 / 80.00f)) * Dog_Move_Max_Height/3);
	}
	else if(Move_Data_Get->Dog_Remote_Control->ch3 > Dog_Move_Control_Error*2)
	{
		*Move_Mode_Set = Front_M;
		*Move_Speed_Get = fabs((float)Move_Data_Get->Dog_Remote_Control->ch3 / 80.00f * Dog_Move_Max_Speed);
		*Move_Lenth_Get = (Dog_Move_Max_Lenth - (1.00f - fabs((float)Move_Data_Get->Dog_Remote_Control->ch3 / 80.00f)) * Dog_Move_Max_Control_Lenth);
		*Move_Height_Get = (Dog_Move_Max_Height - (1.00f - fabs((float)Move_Data_Get->Dog_Remote_Control->ch3 / 80.00f)) * Dog_Move_Max_Height/3);
	}
	//左右转弯
	else if(Move_Data_Get->Dog_Remote_Control->ch2 < -Dog_Move_Control_Error*2)
	{
		*Move_Mode_Set = Left_M;
		*Move_Speed_Get = fabs((float)Move_Data_Get->Dog_Remote_Control->ch2 / 80.00f * Dog_Move_Max_Speed);
		*Move_Lenth_Get = Dog_Move_Max_Lenth - Dog_Move_Max_Control_Lenth;
		*Move_Height_Get = (Dog_Move_Max_Height - (1.00f - fabs((float)Move_Data_Get->Dog_Remote_Control->ch2 / 80.00f)) * Dog_Move_Max_Height/3);
	//	*Move_Lenth_Get = (Dog_Move_Max_Lenth - (1.00f - fabs((float)Motion_Control_Data->Dog_Remote_Control->ch2 / 80.00f)) * Dog_Move_Max_Control_Lenth);
	}
	else if(Move_Data_Get->Dog_Remote_Control->ch2 > Dog_Move_Control_Error*2)
	{
		*Move_Mode_Set = Right_M;
		*Move_Speed_Get = fabs((float)Move_Data_Get->Dog_Remote_Control->ch2 / 80.00f * Dog_Move_Max_Speed);
		*Move_Lenth_Get = Dog_Move_Max_Lenth - Dog_Move_Max_Control_Lenth;
		*Move_Height_Get = (Dog_Move_Max_Height - (1.00f - fabs((float)Move_Data_Get->Dog_Remote_Control->ch2 / 80.00f)) * Dog_Move_Max_Height/3);
	//	*Move_Lenth_Get = (Dog_Move_Max_Lenth - (1.00f - fabs((float)Motion_Control_Data->Dog_Remote_Control->ch2 / 80.00f)) * Dog_Move_Max_Control_Lenth);
	}
	//原地踏步
	else if((Move_Data_Get->Dog_Remote_Control->ch3 < -Dog_Move_Control_Error/2) || (Move_Data_Get->Dog_Remote_Control->ch3 > Dog_Move_Control_Error/2))
	{
		*Move_Mode_Set = Step_M;
		*Move_Speed_Get = fabs((float)Move_Data_Get->Dog_Remote_Control->ch3 / 80.00f * Dog_Move_Max_Speed);
		*Move_Lenth_Get = 0;
		*Move_Height_Get = (Dog_Move_Max_Height - (1.00f - fabs((float)Move_Data_Get->Dog_Remote_Control->ch3 / 80.00f)) * Dog_Move_Max_Height/2);
	}
	else
	{
		*Move_Mode_Set = Stop_M;
		*Move_Speed_Get = 0;
		*Move_Lenth_Get = 0;
		*Move_Height_Get = 0;
	}
	Last_Speed_Control = *Move_Speed_Get;
}

/*******************************************************************************
  * @funtion      : Dog_RM_Dyna_Motion_Mode_Set  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗遥控运动模式控制代码
  * @param         {Lite_Dog_Struct*} Dog_RM_DMM 机器狗数据结构体地址
  * @return        {*}
  *******************************************************************************/
void Dog_RM_Dyna_Motion_Mode_Set(Lite_Dog_Struct* Dog_RM_DMM)
{
	static Dog_Move_Mode_Enum Dog_Move_Mode_Last;	
	Dog_Move_Mode_Enum Dog_Move_Mode_Set;
	Dog_Sport_Mode_Enum Dog_Sport_Mode_Set;
	float Dog_Motion_Set_Speed,Dog_Motion_Set_Lenth,Dog_Motion_Set_Height;

	if(Dog_RM_DMM->Dog_Control_Data.Dog_Remote_Control->s2 == 0x01)
	{
		Dog_Sport_Mode_Set = Trot_M;
	}
	else if(Dog_RM_DMM->Dog_Control_Data.Dog_Remote_Control->s2 == 0x03)
	{
		Dog_Sport_Mode_Set = Walk_M;
	}

	//获取控制值
	Dog_Remote_Motion_Data_Get(&Dog_RM_DMM->Dog_Control_Data,&Dog_Move_Mode_Set,&Dog_Motion_Set_Height,&Dog_Motion_Set_Lenth,&Dog_Motion_Set_Speed);
	//发送控制值
	Dog_Motion_Data_Set(&Dog_RM_DMM->Dog_Control_Data.Dog_Set_Move_Data,Dog_Sport_Mode_Set,Dog_Move_Mode_Set,Dog_Motion_Set_Height,Dog_Motion_Set_Lenth,Dog_Motion_Set_Speed);
	if(Dog_RM_DMM->Dog_Control_Data.Dog_Set_Move_Data.Dog_Move_Mode == Stop_M)
	{
		if(Dog_Move_Mode_Last != Stop_M)
		{
			Lite_Dog_Set_Coordinate(Dog_RM_DMM,Dog_RM_DMM->Dog_Control_Data.Dog_Set_Move_Data.Leg_Stop_XY_Cood[0],Dog_RM_DMM->Dog_Control_Data.Dog_Set_Move_Data.Leg_Stop_XY_Cood[1],Dog_RM_DMM->Dog_Control_Data.Dog_Set_Move_Data.Leg_Stop_XY_Cood[2]);
		}
	}
	else
	{
		if(Dog_Move_Mode_Last == Stop_M)
		{
            Lite_Dog_Get_Coordinate(Dog_RM_DMM,Dog_RM_DMM->Dog_Control_Data.Dog_Set_Move_Data.Leg_Stop_XY_Cood[0],Dog_RM_DMM->Dog_Control_Data.Dog_Set_Move_Data.Leg_Stop_XY_Cood[1],Dog_RM_DMM->Dog_Control_Data.Dog_Set_Move_Data.Leg_Stop_XY_Cood[2]);
		}
		if(Dog_RM_DMM->Dog_Control_Data.Dog_Set_Move_Data.Dog_Move_Mode != Dog_Move_Mode_Last)
		{
			Lite_Dog_Set_Coordinate(Dog_RM_DMM,&Dog_RM_DMM->Dog_Control_Data.Dog_Set_Move_Data.Leg_Stop_XY_Cood[0][0],&Dog_RM_DMM->Dog_Control_Data.Dog_Set_Move_Data.Leg_Stop_XY_Cood[1][0],&Dog_RM_DMM->Dog_Control_Data.Dog_Set_Move_Data.Leg_Stop_XY_Cood[2][0]);		
			Dog_Move_Mode_Enum Dog_Move_Mode_Step = Dog_RM_DMM->Dog_Control_Data.Dog_Set_Move_Data.Dog_Move_Mode;
			Dog_RM_DMM->Dog_Control_Data.Dog_Set_Move_Data.Dog_Move_Mode = Stop_M;
			Dog_Motion_XY_Set(&Dog_RM_DMM->Dog_Control_Data.Dog_Set_Move_Data,Dog_RM_DMM->Dog_Set_XY_Coordinate[1],Dog_RM_DMM->Dog_Set_XY_Coordinate[2]);
			Dog_RM_DMM->Dog_Control_Data.Dog_Set_Move_Data.Dog_Move_Mode = Dog_Move_Mode_Step;
		}
	}
	Dog_Motion_XY_Set(&Dog_RM_DMM->Dog_Control_Data.Dog_Set_Move_Data,Dog_RM_DMM->Dog_Set_XY_Coordinate[1],Dog_RM_DMM->Dog_Set_XY_Coordinate[2]);
	Dog_Move_Mode_Last = Dog_RM_DMM->Dog_Control_Data.Dog_Set_Move_Data.Dog_Move_Mode;
}


/*******************************************************************************
  * @funtion      : Dog_RM_Adaptive_Angle_Mode_Set  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗遥控器控制模式姿态控制状态设置函数
  * @param         {Lite_Dog_Struct*} Dog_RM_AAM
  * @return        {*}
  *******************************************************************************/
void Dog_RM_Adaptive_Angle_Mode_Set(Lite_Dog_Struct* Dog_RM_AAM)
{
	float Pitch_Control_Data,Roll_Control_Data;
    
	Pitch_Control_Data = ((float)Dog_RM_AAM->Dog_Control_Data.Dog_Remote_Control->ch2 / 80.00f * 10.00f);
	Roll_Control_Data = ((float)Dog_RM_AAM->Dog_Control_Data.Dog_Remote_Control->ch2 / 80.00f * 10.00f);
	Lite_Dog_Set_Attitude_Angle(Dog_RM_AAM,Pitch_Control_Data,Roll_Control_Data,0);
    Lite_Dog_Attitude_To_Coordinate(Dog_RM_AAM,Dog_RM_AAM->Dog_Set_XY_Coordinate[0],Dog_RM_AAM->Dog_Set_XY_Coordinate[1],Dog_RM_AAM->Dog_Set_XY_Coordinate[2]);

    // Dog_Att_Data_Set(&Dog_RM_AAM->target.pitch,&Dog_RM_AAM->target.roll,&Dog_RM_AAM->Dog_State_Data,Dog_Y_Add_Data_Get);
	// Dog_RM_AAM->Dog_Set_XY_Coordinate[1][0] += Dog_Y_Add_Data_Get[0];
	// Dog_RM_AAM->Dog_Set_XY_Coordinate[1][1] += Dog_Y_Add_Data_Get[1];
	// Dog_RM_AAM->Dog_Set_XY_Coordinate[1][2] += Dog_Y_Add_Data_Get[2];
	// Dog_RM_AAM->Dog_Set_XY_Coordinate[1][3] += Dog_Y_Add_Data_Get[3];
}

/*******************************************************************************
  * @funtion      : Dog_RM_Att_Control_Mode_Set  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗遥控器控制模式姿态设置
  * @param         {Lite_Dog_Struct*} Dog_RM_AAM
  * @return        {*}
  *******************************************************************************/
void Dog_RM_Att_Control_Mode_Set(Lite_Dog_Struct* Dog_RM_AAM)
{
	float Dog_Y_Add_Data_Get[4];
	float Dog_Y_Pitch_Add;
	float Dog_Y_Roll_Add;
	float Dog_Y_Height_Add;

	Dog_Y_Pitch_Add = (float)Dog_RM_AAM->Dog_Control_Data.Dog_Remote_Control->ch1 / 80.00f * 20.00f;
	Dog_Y_Roll_Add = (float)Dog_RM_AAM->Dog_Control_Data.Dog_Remote_Control->ch0 / 80.00f * 20.00f;
	Dog_Y_Height_Add = (float)Dog_RM_AAM->Dog_Control_Data.Dog_Remote_Control->ch3 / 80.00f * 15.00f + 70.00f;

	Lite_Dog_Set_Attitude_Angle_WithoutMPU(Dog_RM_AAM,Dog_Y_Pitch_Add,Dog_Y_Roll_Add,0);
	Lite_Dog_Set_Attitude_XYZ(Dog_RM_AAM,0,0,Dog_Y_Height_Add);

	Lite_Dog_Attitude_To_Coordinate(Dog_RM_AAM,Dog_RM_AAM->Dog_Set_XY_Coordinate[0],Dog_RM_AAM->Dog_Set_XY_Coordinate[1],Dog_RM_AAM->Dog_Set_XY_Coordinate[2]);

	// Dog_RM_AAM->Dog_Set_XY_Coordinate[1][0] = Dog_Init_Height - Dog_Y_Pitch_Add + Dog_Y_Roll_Add + Dog_Y_Height_Add;
	// Dog_RM_AAM->Dog_Set_XY_Coordinate[1][1] = Dog_Init_Height - Dog_Y_Pitch_Add - Dog_Y_Roll_Add + Dog_Y_Height_Add;
	// Dog_RM_AAM->Dog_Set_XY_Coordinate[1][2] = Dog_Init_Height + Dog_Y_Pitch_Add + Dog_Y_Roll_Add + Dog_Y_Height_Add;
	// Dog_RM_AAM->Dog_Set_XY_Coordinate[1][3] = Dog_Init_Height + Dog_Y_Pitch_Add - Dog_Y_Roll_Add + Dog_Y_Height_Add;

	// All_Leg_XY_Data_Check(Dog_RM_AAM);
	// Dog_All_Legs_Control_Data_Send(Dog_RM_AAM,0,0);
}



/*******************************************************************************
  * @funtion      : Dog_Remote_Control_Mode_Set 
  * @LastEditors  : JackyJuu
  * @description  : 机器狗遥控器模式不同状态设置函数
  * @param         {Dog_Control_Data_Struct*} Dog_Control_Data
  * @return        {*}
  *******************************************************************************/
void Dog_Remote_Control_Mode_Set(Dog_Control_Data_Struct* Dog_Control_Data)
{
	//S1下，S2下，舵机无力模式
	if((Dog_Control_Data->Dog_Remote_Control->s1 == 0x02) && (Dog_Control_Data->Dog_Remote_Control->s2 == 0x02))
	{
		Dog_Control_Data->Dog_Remote_Mode = Dog_RM_PowerLess_Mode;
	}
	//S1中，S2中，姿态控制模式
	else if((Dog_Control_Data->Dog_Remote_Control->s1 == 0x03) && (Dog_Control_Data->Dog_Remote_Control->s2 == 0x03))
	{
		Dog_Control_Data->Dog_Remote_Mode = Dog_RM_Att_Control_Mode;
	}
	//S1中，S2上，自适应角度模式
	else if((Dog_Control_Data->Dog_Remote_Control->s1 == 0x03) && (Dog_Control_Data->Dog_Remote_Control->s2 == 0x01))
	{
		Dog_Control_Data->Dog_Remote_Mode = Dog_RM_Adaptive_Angle_Mode;
	}
	//S1上，S2下，舵机固定角度模式
	else if((Dog_Control_Data->Dog_Remote_Control->s1 == 0x01) && (Dog_Control_Data->Dog_Remote_Control->s2 == 0x02))
	{
		Dog_Control_Data->Dog_Remote_Mode = Dog_RM_Att_Control_Mode;
	}
	//S1上，S2中，动态运动模式,WALK
	else if((Dog_Control_Data->Dog_Remote_Control->s1 == 0x01) && (Dog_Control_Data->Dog_Remote_Control->s2 == 0x03))
	{
		Dog_Control_Data->Dog_Remote_Mode = Dog_RM_Dyna_Motion_Mode;
	}
	//S1上，S2上，动态运动模式,TROT
	else if((Dog_Control_Data->Dog_Remote_Control->s1 == 0x01) && (Dog_Control_Data->Dog_Remote_Control->s2 == 0x01))
	{
		Dog_Control_Data->Dog_Remote_Mode = Dog_RM_Dyna_Motion_Mode;
	}	
}


/*******************************************************************************
  * @funtion      : Dog_Remote_Control_Mode_Update  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗遥控器模式不同状态更新函数
  * @param         {Lite_Dog_Struct*} Dog_Control_Data_Update
  * @return        {*}
  *******************************************************************************/
void Dog_Remote_Control_Mode_Update(Lite_Dog_Struct* Dog_Control_Data_Update)
{
	switch(Dog_Control_Data_Update->Dog_Control_Data.Dog_Remote_Mode)
	{
		case Dog_RM_Adaptive_Angle_Mode:
			Dog_Control_Data_Update->Dog_State = Dog_OK;
			Dog_RM_Adaptive_Angle_Mode_Set(Dog_Control_Data_Update);
		break;
		case Dog_RM_Att_Control_Mode:
			Dog_Control_Data_Update->Dog_State = Dog_OK;
			Dog_RM_Att_Control_Mode_Set(Dog_Control_Data_Update);
		break;
		case Dog_RM_Dyna_Motion_Mode:
			Dog_Control_Data_Update->Dog_State = Dog_OK;
			Dog_RM_Dyna_Motion_Mode_Set(Dog_Control_Data_Update);
		break;
		case Dog_RM_Celebrate_Mode:
			
		break;
		case Dog_RM_PowerLess_Mode:
			Lite_Dog_Set_PowerLess();
		break;
	}
}



